#include	"tank.h"
#define PI 3.1415926535897

void pre_process(void);

struct node
{
    float x;
    float y;
    int property;
    double angle; 
}info_node[MAX_VERTEX>>1];

void
load_node()
{
	int i;
	for(i = 0; i < vertex_count; i++) {
		if(i % 2 == 0) {
			info_node[i/2+1].x = mapvertex[i];
		} else {
			info_node[i/2+1].y = mapvertex[i];
		}
	}
	return;
}

void read_map(char mapname[],int max)
{
	FILE *fp;
	fp = fopen(mapname,"r");
	float fdxs;
	int num_fdxs=0;
	while (fscanf(fp,"%f",&fdxs)!=EOF) {
		mapvertex[num_fdxs++]=fdxs;
	}
	vertex_count = num_fdxs;
	load_node();
	pre_process();
	return;
}

void pre_process()
{
	int i,num_node;
	num_node=vertex_count>>1;
	for (i=3;i<=num_node-2;i++)
	{
		//special judge
		if (i==3)
		{
			if (info_node[i+1].y>info_node[i].y)
				info_node[i].property=1;
			if (info_node[i+1].y<info_node[i].y)
				info_node[i].property=3;
			info_node[i].angle=atan(((double) (info_node[i+1].y-info_node[i].y))/(info_node[i+1].x-info_node[i].x));
			continue;
		}
		if (i==num_node-2)
		{
			if (info_node[i].y>info_node[i-1].y)
				info_node[i].property=2;
			if (info_node[i].y<info_node[i-1].y)
				info_node[i].property=4;
			info_node[i].angle=atan(((double) (info_node[i].y-info_node[i-1].y))/(info_node[i].x-info_node[i-1].x));
			break;
		}
		//property
		if (info_node[i-1].y==info_node[i].y && info_node[i+1].y>info_node[i].y)
			info_node[i].property=1;
		if (info_node[i-1].y<info_node[i].y && info_node[i+1].y==info_node[i].y)
			info_node[i].property=2;
		if (info_node[i-1].y==info_node[i].y && info_node[i+1].y<info_node[i].y)
			info_node[i].property=3;
		if (info_node[i-1].y>info_node[i].y && info_node[i+1].y==info_node[i].y)
			info_node[i].property=4;
		//angle
		if (info_node[i].property==1 || info_node[i].property==3)
			info_node[i].angle=atan(((double) (info_node[i+1].y-info_node[i].y))/(info_node[i+1].x-info_node[i].x));
		if (info_node[i].property==2 || info_node[i].property==4)
			info_node[i].angle=atan(((double) (info_node[i].y-info_node[i-1].y))/(info_node[i].x-info_node[i-1].x));
	}
	return ;
}

int find_node(float x_t)
{
	int left=3,right=(vertex_count>>1)-2,mid;
	while (left<right)
	{
		mid=(left+right)>>1;
		if (x_t==info_node[mid].x)
			return mid;
		if (x_t<info_node[mid].x)
			right=mid;
		if (x_t>info_node[mid].x)
			left=mid+1;
	}
	return left;
}

void tank_pos(float length, float x, float *y, double *angle)
{
	if (x<=info_node[2].x)
	{
		*y=info_node[2].y;
		*angle=info_node[2].angle;
		return ;
	}
	if (x>=info_node[(vertex_count>>1)-2].x)
	{
		*y=info_node[(vertex_count>>1)-1].y;
		*angle=info_node[(vertex_count>>1)-1].angle;
		return ;
	}
	int xb;
	xb=find_node(x);
	double cita=info_node[xb].angle;
	float dis=fabs(info_node[xb].x-x);
	double p_len,k_len,ang = 0.0;
	if (info_node[xb].property==1)
	{ 
		if (dis>=length)
		{
			*y=info_node[xb].y;
			ang=0;
		}
		else 
		{
			*y=info_node[xb].y;
			ang=cita-asin(dis*sin(cita)/length);
		}
	}
	if (info_node[xb].property==3)
	{
		if (dis>=(length/2))
		{
			*y=info_node[xb].y;
			ang=0;
		}
		else
		{
			p_len=(length/2)*cos(cita);
			if (dis<=p_len)
			{
				*y=info_node[xb].y-dis*tan(cita);
				ang=cita;
			}
			else 
			{
				k_len=(length/2);
				p_len=sqrt((k_len)*(k_len)-(dis)*(dis));
				*y=info_node[xb].y+p_len;
				ang=(-1)*asin(p_len/k_len);
			}
		}
	}
	if (info_node[xb].property==2)
	{
		if ((dis/cos(cita))>=(length/2))
		{
			*y=info_node[xb].y-dis*tan(cita);
			ang=cita;
		}
		else
		{
			p_len=(length/2)*cos(cita);
			k_len=dis/p_len;
			ang=k_len*cita;
			p_len=dis*tan(ang);
			*y=info_node[xb].y-p_len;
		}
	}
	if (info_node[xb].property==4)
	{
		if ((dis/cos(cita))>=length)
		{
			*y=info_node[xb].y-dis*tan(cita);
			ang=cita;
		}	
		else
		{
			*y=info_node[xb].y-dis*tan(cita);
			ang=asin(dis*tan(cita)/length);
		}
	}
	*angle=ang/PI*180;
    return ;
}
